{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# Camera Resectioning Example\n",
        "\n",
        "This is a 1:1 transcription of CameraResectioning.cpp, but using custom factors."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "colab_import",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n    import google.colab\n    %pip install --quiet gtsam-develop\nexcept ImportError:\n    pass"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n",
        "Atlanta, Georgia 30332-0415\n",
        "All Rights Reserved\n",
        "\n",
        "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
        "\n",
        "See LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/CameraResectioning.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "tags": [
          "remove-cell"
        ],
        "vscode": {
          "languageId": "markdown"
        }
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {},
      "outputs": [],
      "source": [
        "import numpy as np\n",
        "from gtsam import Cal3_S2, CustomFactor, LevenbergMarquardtOptimizer, KeyVector\n",
        "from gtsam import NonlinearFactor, NonlinearFactorGraph\n",
        "from gtsam import PinholeCameraCal3_S2, Point2, Point3, Pose3, Rot3, Values\n",
        "from gtsam.noiseModel import Base as SharedNoiseModel, Diagonal\n",
        "from gtsam.symbol_shorthand import X"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "def resectioning_factor(\n",
        "    model: SharedNoiseModel,\n",
        "    key: int,\n",
        "    calib: Cal3_S2,\n",
        "    p: Point2,\n",
        "    P: Point3,\n",
        ") -> NonlinearFactor:\n",
        "\n",
        "    def error_func(this: CustomFactor, v: Values, H: list[np.ndarray]) -> np.ndarray:\n",
        "        pose = v.atPose3(this.keys()[0])\n",
        "        camera = PinholeCameraCal3_S2(pose, calib)\n",
        "        if H is None:\n",
        "            return camera.project(P) - p\n",
        "        Dpose = np.zeros((2, 6), order=\"F\")\n",
        "        result = camera.project(P, Dpose) - p\n",
        "        H[0] = Dpose\n",
        "        return result\n",
        "\n",
        "    return CustomFactor(model, KeyVector([key]), error_func)\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "Assumptions:\n",
        "- Camera: $f = 1$, Image: $100\\times100$, center: $50, 50.0$\n",
        "- Pose (ground truth): $(X_w, -Y_w, -Z_w, [0,0,2.0]^T)$\n",
        "- Known landmarks: $(10,10,0), (-10,10,0), (-10,-10,0), (10,-10,0)$\n",
        "- Perfect measurements: $(55,45), (45,45),  (45,55), (55,55)$\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Final result:\n",
            "\n",
            "Values with 1 values:\n",
            "Value x1: (gtsam::Pose3)\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, -1, 0;\n",
            "\t0, 0, -1\n",
            "]\n",
            "t: 0 0 2\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Create camera intrinsic parameters\n",
        "calibration = Cal3_S2(1, 1, 0, 50, 50)\n",
        "\n",
        "# 1. create graph\n",
        "graph = NonlinearFactorGraph()\n",
        "\n",
        "# 2. add factors to the graph\n",
        "measurement_noise = Diagonal.Sigmas(np.array([0.5, 0.5]))\n",
        "graph.add(\n",
        "    resectioning_factor(\n",
        "        measurement_noise, X(1), calibration, Point2(55, 45), Point3(10, 10, 0)\n",
        "    )\n",
        ")\n",
        "graph.add(\n",
        "    resectioning_factor(\n",
        "        measurement_noise, X(1), calibration, Point2(45, 45), Point3(-10, 10, 0)\n",
        "    )\n",
        ")\n",
        "graph.add(\n",
        "    resectioning_factor(\n",
        "        measurement_noise, X(1), calibration, Point2(45, 55), Point3(-10, -10, 0)\n",
        "    )\n",
        ")\n",
        "graph.add(\n",
        "    resectioning_factor(\n",
        "        measurement_noise, X(1), calibration, Point2(55, 55), Point3(10, -10, 0)\n",
        "    )\n",
        ")\n",
        "\n",
        "# 3. Create an initial estimate for the camera pose\n",
        "initial: Values = Values()\n",
        "initial.insert(X(1), Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 1)))\n",
        "\n",
        "# 4. Optimize the graph using Levenberg-Marquardt\n",
        "result: Values = LevenbergMarquardtOptimizer(graph, initial).optimize()\n",
        "result.print(\"Final result:\\n\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 2
}